#include "at91sam7s.h"

#include "jtag.h"
#include "jtag_pins.h"

/* WARNING: the _dx() form of the macros requires TDI to be bit 0 */
#define TMS0 0
#define TMS1 PIN_TMS
#define TCK0 0
#define TCK1 PIN_TCK
#define TDI0 0
#define TDI1 PIN_TDI

#define next_m0_d1() (pio->data_status = TCK0 | TMS0 | TDI1,    \
                      pio->data_status = TCK1 | TMS0 | TDI1)
#define next_m1_d1() (pio->data_status = TCK0 | TMS1 | TDI1,    \
                      pio->data_status = TCK1 | TMS1 | TDI1)
#define next_m0_d0() (pio->data_status = TCK0 | TMS0 | TDI0,    \
                      pio->data_status = TCK1 | TMS0 | TDI0)
#define next_m1_d0() (pio->data_status = TCK0 | TMS1 | TDI0,    \
                      pio->data_status = TCK1 | TMS1 | TDI0)
#define next_m0_dx(x) (pio->data_status = TCK0 | TMS0 | (x),    \
                       pio->data_status = TCK1 | TMS0 | (x))
#define next_m1_dx(x) (pio->data_status = TCK0 | TMS1 | (x),    \
                       pio->data_status = TCK1 | TMS1 | (x))

#define sense()  (pio->pin_status & PIN_TDO)

void jtag_reset(void)
{
    AT91PIO *pio = AT91PIO_ADDR;
    next_m1_d1();
    next_m1_d1();
    next_m1_d1();
    next_m1_d1();
    next_m1_d1();
    next_m1_d1();
    next_m1_d1();
    next_m1_d1(); /* ... -> reset (IDCODE selected) */
    next_m0_d1(); /* reset -> run-test-idle */
    next_m1_d1(); /* run-test-idle -> select-dr */
}

static void jtag_write_ir(AT91PIO *pio, unsigned val, int idle)
{
    next_m1_d1(); /* select-dr -> select-ir */
    next_m0_d1(); /* select-ir -> capture-ir */
    next_m0_d1(); /* capture-ir -> shift-ir */
    
    next_m0_dx((val & 1)); /* shift-ir -> shift->ir */
    val >>= 1;
    
    next_m0_dx((val & 1)); /* shift-ir -> shift->ir */
    val >>= 1;
    
    next_m0_dx((val & 1)); /* shift-ir -> shift->ir */
    val >>= 1;
    
    next_m1_dx((val & 1)); /* shift-ir -> exit->ir */
    val >>= 1;
    
    next_m1_d1(); /* exit1-ir -> update-ir */
    
    if(idle) {
        next_m0_d1(); /* update-ir -> run-test-idle */
        next_m1_d1(); /* run-test-idle -> select-dr */
    } else {
        next_m1_d1(); /* update-ir -> select-dr */
    }
}

static void jtag_write_dr(AT91PIO *pio, unsigned out, unsigned count)
{
    next_m0_d1(); /* select-dr -> capture-dr */
    next_m0_d1(); /* capture-dr -> shift-dr */

    while(--count > 0){
        next_m0_dx(out & 1); /* shift-dr -> shift-dr */
        out >>= 1;
    }

    next_m1_dx(out & 1); /* shift-dr -> exit1-dr */
    
    next_m1_d1(); /* exit1-dr -> update-dr */
    next_m1_d1(); /* update-dr -> select-dr */
}

static unsigned jtag_read_dr_32(AT91PIO *pio)
{
    unsigned in = 0;
    unsigned count;
    unsigned bit = 1;
    
    next_m0_d1(); /* select-dr -> capture-dr */
    next_m0_d1(); /* capture-dr -> shift-dr */
    
    for(count = 0; count < 31; count++) {
        next_m0_d1();
        in |= (sense() ? bit : 0);
        bit <<= 1;
    }

    next_m1_d1(); /* shift-dr -> exit1-dr */
    in |= (sense() ? bit : 0);

    next_m1_d1(); /* exit1-dr -> update-dr */
    next_m1_d1(); /* update-dr -> select-dr */

    return in;
}


unsigned jtag_dbg_io(unsigned out, unsigned bkpt)
{
    AT91PIO *pio = AT91PIO_ADDR;
    unsigned count;
    unsigned in = 0;
    unsigned bit = 0x80000000;
    
    next_m0_d1(); /* select-dr -> capture-dr */
    next_m0_d1(); /* capture-dr -> shift-dr */

    if(bkpt) {
        next_m0_d1(); /* shift-dr -> shift-dr */
    } else {
        next_m0_d0(); /* shift-dr -> shift-dr */
    }
    
    for(count = 0; count < 31; count++){
        if(out & bit) {
            next_m0_d1(); /* shift-dr -> shift-dr */
        } else {
            next_m0_d0(); /* shift-dr -> shift-dr */
        }
        if(sense()) in |= bit;
        bit >>= 1;
    }

    if(out & bit) {
        next_m1_d1(); /* shift-dr -> exit1-dr */
    } else {
        next_m1_d0(); /* shift-dr -> exit1-dr */
    }
    if(sense()) in |= bit;

    next_m1_d1(); /* exit1-dr -> update-dr */
    next_m0_d1(); /* update-dr -> run-test */
    next_m1_d1(); /* run-test -> select-dr */

    return in;
}

/* ICE Registers:  TDI -> RW(1) -> REG(5) -> DATA(32) -> TDO */

void jtag_ice_wr(unsigned reg, unsigned out)
{
    AT91PIO *pio = AT91PIO_ADDR;
    unsigned count;
    
    next_m0_d1(); /* select-dr-scan -> capture-dr */
    next_m0_d1(); /* capture-dr -> shift-dr */

        /* shift the data in first */
    for(count = 0; count < 31; count++) {
        next_m0_dx(out & 1);
        out >>= 1;
    }
    next_m0_dx(out & 1);

        /* shift the register number next */
    for(count = 0; count < 5; count++){
        next_m0_dx(reg & 1);
        reg >>= 1;
    }

        /* shift the RW bit (1 = write) last */
    next_m1_d1(); /* shift-dr -> exit1-dr */
    next_m1_d1(); /* exit1-dr -> update-dr */
    next_m1_d1(); /* update-dr -> select-dr */    
}

unsigned jtag_ice_rd(unsigned reg)
{
    AT91PIO *pio = AT91PIO_ADDR;
    unsigned in = 0;
    unsigned bit = 1;
    unsigned count;
    
    next_m0_d1(); /* select-dr-scan -> capture-dr */
    next_m0_d1(); /* capture-dr -> shift-dr */

        /* shift the register id */
    next_m0_dx(reg & 1); /* shift-dr -> shift-dr */
    reg >>= 1;
    next_m0_dx(reg & 1); /* shift-dr -> shift-dr */
    reg >>= 1;
    next_m0_dx(reg & 1); /* shift-dr -> shift-dr */
    reg >>= 1;
    next_m0_dx(reg & 1); /* shift-dr -> shift-dr */
    reg >>= 1;
    next_m0_dx(reg & 1); /* shift-dr -> shift-dr */

        /* shift the RW bit (0 = read) */
    next_m1_d0(); /* shift-dr -> exit1-dr */
    next_m1_d1(); /* exit1-dr -> update-dr */
    next_m1_d1(); /* update-dr -> select-dr */    
    next_m0_d1(); /* select-dr -> capture-dr */
    next_m0_d1(); /* capture-dr -> shift-dr */

    for(count = 0; count < 31; count++){
        next_m0_d1(); /* shift-dr -> shift-dr */
        in |= (sense() ? bit : 0);
        bit <<= 1;
    }

    next_m1_d0(); /* shift-dr -> exit1-dr */
    in |= (sense() ? bit : 0);
    
    next_m1_d1(); /* exit1-dr -> update-dr */
    next_m1_d1(); /* update-dr -> select-dr */

    return in;
}

void jtag_select_ice(void)
{
    AT91PIO *pio = AT91PIO_ADDR;
    jtag_write_ir(pio, IR_SCAN_N, 0);
    jtag_write_dr(pio, 2, 4);
    jtag_write_ir(pio, IR_INTEST, 0);
}

void jtag_select_debug(void)
{
    AT91PIO *pio = AT91PIO_ADDR;
    jtag_write_ir(pio, IR_SCAN_N, 0);
    jtag_write_dr(pio, 1, 4);
    jtag_write_ir(pio, IR_INTEST, 0);
}

void jtag_select_restart(void)
{
    AT91PIO *pio = AT91PIO_ADDR;
    jtag_write_ir(pio, IR_RESTART, 1);
}

unsigned jtag_get_id(void)
{
    AT91PIO *pio = AT91PIO_ADDR;
    jtag_reset();
    return jtag_read_dr_32(pio);
}

void jtag_init(void)
{
    AT91PIO *pio = AT91PIO_ADDR;
    AT91PMC *pmc = AT91PMC_ADDR;
    
        /* enable MCK to PIOA so that input lines work */
    pmc->PCER = (1 << PID_PIOA);
    
          /* make sure these are high before we make them outputs,
           so we don't get any glitches during startup */
    pio->data_set = PIN_TDI | PIN_TMS | PIN_TCK | PIN_nSRST;
    pio->output_enable = PIN_TDI | PIN_TMS | PIN_TCK | PIN_nSRST;
    
        /* we want to be able to update all 3 of the JTAG
           output bits with one write */
    pio->write_disable = 0xffffffff;
    pio->write_enable = PIN_TDI | PIN_TMS | PIN_TCK;

    pio->pullup_disable = PIN_TDI | PIN_TMS | PIN_TCK | PIN_TDO | PIN_nSRST;
    pio->pio_enable =  PIN_TDI | PIN_TMS | PIN_TCK | PIN_TDO | PIN_nSRST;
}

void jtag_srst(unsigned hi)
{
    AT91PIO *pio = AT91PIO_ADDR;

    if(hi) {
        pio->data_set = PIN_nSRST;
    } else {
        pio->data_clear = PIN_nSRST;
    }
}
